#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <ros/ros.h>
#include <opencv2/opencv.hpp>
#include <thread>
#include "commons/INIReader.h"
#include "commons/cmdline.h"
#include "commons/libar8020.h"
#include "commons/rabbit_consts.h"
#include "commons/timer.h"
#include "commons/x264_encoder.h"


void imageCallback(const sensor_msgs::ImageConstPtr& msg) {
  x264Encoder endocer(BIG_VIDEO_HIGHT, BIG_VIDEO_WIDTH, 3, 15);
  try {
    cv::Mat frame = cv_bridge::toCvShare(msg, "bgr8")->image;
    int size = endocer.EncodeOneFrame(frame);
    uchar* data = nullptr;
    int res = -1;
    data = endocer.GetEncodedFrame();
    try {
      res = Video_Port_Send_So((char*)data, size);
      std::cout<< "Send h264 with res " << res<< "\r\n"; 
      //LOG(INFO) << "Send h264 with res " << res;
      } catch (...) {
       // LOG(INFO) << "Error send h264";
      }
    //cv::imshow("view", cv_bridge::toCvShare(msg, "bgr8")->image);

  } catch (cv_bridge::Exception& e) {
    ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
  }
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "ai_video_send");
  ros::NodeHandle nh;
  while (Usb_Init_So() < 0) {        //图传模块初始化
    LOG(INFO) << "libar8020 usb init error, reinit in 5 secs";
    std::this_thread::sleep_for(std::chrono::seconds(5));
  }
  image_transport::ImageTransport it(nh);
  image_transport::Subscriber sub =it.subscribe(TOPIC_CAMERA, 1, imageCallback);  //TOPIC_CAMERA_AI

  ros::spin();
}
